#include "ksem.h"
#include "semaphores.h"
#include "scheduler.h"
#include "os_io.h"


extern pcb_t *g_running_process;

void lock_ksem(uint32_t data)
{
    pcb_t *cur_pcb;
    sem_t *sem_ptr = (sem_t *)data;

    sem_ptr->err_code = ERR_SUCCESS;
    if (sem_ptr->sem_lock == SEM_UNLOCKED)
    {
	sem_ptr->sem_lock = SEM_LOCKED;
	return;
    }

    // Semaphore already locked. Block process.
    cur_pcb = g_running_process;
    sem_ptr->process_list.prev_process->next_process = &cur_pcb->blocked_process;
    cur_pcb->blocked_process.prev_process = sem_ptr->process_list.prev_process;
    cur_pcb->blocked_process.next_process = &sem_ptr->process_list;
    sem_ptr->process_list.prev_process = &cur_pcb->blocked_process;

    cur_pcb->blocked_res = sem_ptr;
    cur_pcb->process_state = process_waiting;
    //printf("%s switched to waiting state\n", cur_pcb->name);
    //printf("0x%x added to the semaphore queue\n", &cur_pcb->blocked_process);

    return;
}


void ksem_remove_process_from_queue(pcb_t *process)
{
    //printf("0x%x removed from the queue\n", (uint32_t)&process->blocked_process);
    process->blocked_process.prev_process->next_process = process->blocked_process.next_process;
    process->blocked_process.next_process->prev_process = process->blocked_process.prev_process;
    return;
}

void unlock_ksem(uint32_t data)
{
    sem_t *sem_ptr = (sem_t *)data;
    pcb_t *next_process;

    sem_ptr->err_code = ERR_SUCCESS;
    if ((sem_ptr->process_list.next_process) == &sem_ptr->process_list) {
	// No other processes were blocked on semaphore
	sem_ptr->sem_lock = SEM_UNLOCKED;
	return;
    }

    next_process = (pcb_t *)((uint32_t)sem_ptr->process_list.next_process - 
                                   OFFSET_OF(blocked_process, pcb_t));
    ksem_remove_process_from_queue(next_process);


    next_process->blocked_res = NULL;
    add_process_to_ready_queue(next_process);
    //printf("0x%x added to ready queue\n", (uint32_t)next_process);

    return;
}

void trylock_ksem(uint32_t data)
{
    sem_t *trylock_sem = (sem_t *)data;
    if (trylock_sem->sem_lock == SEM_LOCKED) {
	trylock_sem->err_code = ERR_BUSY;
    } else {
	lock_ksem((uint32_t)trylock_sem);
    }

    return;
}

